#define SELECTION_RANGE 10

float protected_div(float _a, float _b) {
	if (_b != 0.0)
		return _a/_b;
	else return 0;
}

void upshift_matrix(__global float* _M, int _n, int _startcol) {
	int n = _n;

	for (int i = _startcol; i < n-1; i++)
		for (int j = 0; j <= n; j++)
			_M[(n+1)*i+j] = _M[(n+1)*(i+1)+j];
	for (int j = 0; j <= n; j++)
		_M[(n+1)*(n-1)+j] = 0.0;

	return;
}

__kernel void reduce(__global float* _eval, int _n_cases, int _n_genes) {
	int i = get_global_id(0);

	int mod = 1;
	float cases_rem = (float) _n_cases;
	while (cases_rem > 1) {
		if (i%(mod*2) != 0)
			return;
		if (i + mod < _n_cases)
			_eval[i*_n_genes] += _eval[(i+mod)*_n_genes];

		mod *= 2;
		cases_rem *= 0.5;
		barrier(CLK_GLOBAL_MEM_FENCE);
	}

	return;
}

__kernel void compile(__global char* _pool, __global uchar4* _assembly, int _len, int _genes) {
	int i = get_global_id(0);	// per chromosome
	global char*	source = _pool + _len * i;
	global uchar4* 	dest = _assembly + (_len + _genes) * i; 

	int geneLen = _len/_genes;

	int read_ptr = 0;
	int write_ptr = 1;
	int head_ptr = 0;
	char addr_ptr = 2;

	int genes_remain = _genes;
	while (genes_remain--) {
		int symbols_remain = 1;
		while (symbols_remain--) {
			int n_args = 0;
			switch(source[read_ptr]) {
				case '+':
				dest[write_ptr].x = 1;
				dest[write_ptr].y = addr_ptr++;
				dest[write_ptr].z = addr_ptr++;
				dest[write_ptr].w = 0;
				n_args = 2;
				break;
				case '-':
				dest[write_ptr].x = 2;
				dest[write_ptr].y = addr_ptr++;
				dest[write_ptr].z = addr_ptr++;
				dest[write_ptr].w = 0;
				n_args = 2;
				break;
				case '*':
				dest[write_ptr].x = 3;
				dest[write_ptr].y = addr_ptr++;
				dest[write_ptr].z = addr_ptr++;
				dest[write_ptr].w = 0;
				n_args = 2;
				break;
				case '/':
				dest[write_ptr].x = 4;
				dest[write_ptr].y = addr_ptr++;
				dest[write_ptr].z = addr_ptr++;
				dest[write_ptr].w = 0;
				n_args = 2;
				break;
				case 'Q':
				dest[write_ptr].x = 5;
				dest[write_ptr].y = addr_ptr++;
				dest[write_ptr].z = 0;
				dest[write_ptr].w = 0;
				n_args = 1;
				break;
				case 'x':
				dest[write_ptr].x = 128;
				dest[write_ptr].y = 0;
				dest[write_ptr].z = 0;
				dest[write_ptr].w = 0;
				n_args = 0;
				break;
				default:
				dest[write_ptr].x = 86;
				dest[write_ptr].y = 86;
				dest[write_ptr].z = 86;
				dest[write_ptr].w = 86;
				n_args = 0;
				break;
			}
			symbols_remain += n_args;
			read_ptr += 1;
			write_ptr += 1;
		}
		dest[head_ptr].x = write_ptr-1;
		dest[head_ptr].y = genes_remain? 1: 0;
		dest[head_ptr].z = 0;
		dest[head_ptr].w = 0;

		head_ptr = write_ptr++;
		read_ptr = geneLen * (_genes - genes_remain);
		addr_ptr += 2;
	}

	return;
}

__kernel void parse(__global uchar4* _assembly, int _prog, int _len, __constant float* _training, __global float* _eval, int _args, int _genes) {
	int i = get_global_id(0);	// per fitness case
	__global uchar4* 	source = _assembly + _prog * (_len + _genes);
	__global float* 	result = _eval + i * _genes;
	__constant float* 	inputs = _training + i * (_args+1);
	//float 			output = _training[i * (_args+1) + _args];
	float			wrkspc[1024];

	int genes_remain = _genes;
	int read_ptr = source[0].x;
	int stop_ptr = 0;
	while (genes_remain) {
		for (; read_ptr > stop_ptr; read_ptr--) {
			switch (source[read_ptr].x) {
				case 1: {
					float a, b;
					a = wrkspc[source[read_ptr].y];
					b = wrkspc[source[read_ptr].z];
					wrkspc[read_ptr] = a+b;
				}
				break;
				case 2: {
					float a, b;
					a = wrkspc[source[read_ptr].y];
					b = wrkspc[source[read_ptr].z];
					wrkspc[read_ptr] = a-b;
				}
				break;
				case 3: {
					float a, b;
					a = wrkspc[source[read_ptr].y];
					b = wrkspc[source[read_ptr].z];
					wrkspc[read_ptr] = a*b;
				}
				break;
				case 4: {
					float a, b;
					a = wrkspc[source[read_ptr].y];
					b = wrkspc[source[read_ptr].z];
					wrkspc[read_ptr] = protected_div(a,b);
				}
				break;
				case 5: {
					float a;
					a = wrkspc[source[read_ptr].y];
					if (a < 0)
						wrkspc[read_ptr] = -sqrt(fabs(a));
					else
						wrkspc[read_ptr] = sqrt(a);
				}
				break;
				case 128: {
					wrkspc[read_ptr] = inputs[0];
				}
				break;
			}
		}
		result[_genes - genes_remain] = wrkspc[stop_ptr+1];
		genes_remain -= 1;
		stop_ptr = source[read_ptr].x + 1;
		read_ptr = source[source[read_ptr].x + 1].x;
	}

	return;
}

__kernel void build_MLS_entry(__global float* _result, __global float* _training, __global float* _MLS, int _n_args, int _n_cases, int _n_genes, int _row, int _col) {
	int i = get_global_id(0); 	// per fitness case

	__local float squares[4096];

	if (_col < _n_genes) 
		squares[i] = _result[i * _n_genes + _col] * _result[i * _n_genes + _row];
	else
		squares[i] = _training[i * (_n_args+1) + _n_args] * _result[i * _n_genes + _row];
	barrier(CLK_LOCAL_MEM_FENCE);

	int mod = 1;
	float cases_rem = (float) _n_cases;
	while (cases_rem > 1) {
		if (i%(mod*2) != 0)
			return;
		if (i + mod < _n_cases)
			squares[i] += squares[i + mod];

		mod *= 2;
		cases_rem *= 0.5;
		barrier(CLK_LOCAL_MEM_FENCE);
	}

	if (i == 0)
		_MLS[_row * (_n_genes+1) + _col] = squares[0];

	return;
}

__kernel void QR_factor(__global float* _MLS, int _n_genes) {
	int i = get_global_id(0); 	// per annihilated entry

	int n = _n_genes;
	int j = n-1;
	int k = 0;

	if (i == 0) {
		int r = 0;
		int s = 0;
		while (s < n-1) {
			bool row_is_zero = true;
			for (int c = 0; c < n; c++)
				if (fabs(_MLS[(n+1)*r + c]) > 1e-04)
					row_is_zero = false;
			if (row_is_zero)
				upshift_matrix(_MLS, _n_genes, r);
			else
				r++;
			s++;
		}
	}
	barrier(CLK_GLOBAL_MEM_FENCE);


	int t = 0;
	factor:
	while (k < n) {
		while (j > k) {
			if (j+2*i < n) {
				float a, b;
				float r, c, s;
				a = _MLS[(n+1)*(j+2*i-1) + (k+i) + t];
				b = _MLS[(n+1)*(j+2*i  ) + (k+i) + t];
				if (fabs(b) < 1e-04)
					goto short_circuit;
				r = sqrt(a*a + b*b);
				c = a/r;
				s = b/r;
				for (int q = k+i+t; q <= n; q++) {
					float u, v;
					u = c * _MLS[(n+1)*(j+2*i-1) + q]
						+ s * _MLS[(n+1)*(j+2*i) + q];
					v = c * _MLS[(n+1)*(j+2*i) + q]
						- s * _MLS[(n+1)*(j+2*i-1) + q];
					_MLS[(n+1)*(j+2*i-1) + q] = u;
					_MLS[(n+1)*(j+2*i  ) + q] = v;
				}
			}
			short_circuit:
			barrier(CLK_GLOBAL_MEM_FENCE);
			j--;
		}
		k++;
		j = k + 1;
	}
	if (fabs(_MLS[t]) < 1e-04 && t < n) {
		t++;
		k = 0;
		j = t;
		goto factor;
	}

	return;
}

__kernel void back_substitute(__global float* _MLS, __global float* _coeffs, int _n_genes) {
	int i = get_global_id(0);

	__local bool solved_mask[128];
	volatile __local int solved_equations;

	if (i == 0)
		solved_equations = 0;
	barrier(CLK_LOCAL_MEM_FENCE);

	int n = _n_genes;

	bool column_is_zero = true;
	for (int j = 0; j < n; j++)
		if ( fabs(_MLS[(n+1)*j + i]) > 1e-04)
			column_is_zero = false;
	if (column_is_zero) {
		_coeffs[i] = 0;
		atomic_inc(&solved_equations);
	}
	solved_mask[i] = column_is_zero;
	barrier(CLK_LOCAL_MEM_FENCE);

	if (i != 0)
		return;
	
	for (int j = n-1 - solved_equations; j >= 0; j--) {
		int q = n-1;
		float coeff = _MLS[ (n+1)*j + n ];
		while (solved_mask[q] && q >= 0) {
			float term = _MLS[ (n+1)*j + q ] * _coeffs[q];
			coeff -= term;
			q--;
		}
		_coeffs[q] = protected_div(coeff, _MLS[ (n+1)*j + q]);
		solved_mask[q] = true;
	}

	return;
}

__kernel void get_fitness(__global float* _eval, __global float* _coeffs, global float* _trainData, __global float* _fitness, int _n_cases, int _n_genes, int _n_args, int _prog) {
	int i = get_global_id(0);

	float term = 0;
	for (int g = 0; g < _n_genes; g++) 
		term += _coeffs[g]*_eval[_n_genes*i + g];

	float target = _trainData[i*(_n_args+1)+_n_args];
	float fitness = SELECTION_RANGE - fabs(protected_div( target - term, target )) * 100;
	if (fitness < 0.0)
		fitness = 0.0;
	_eval[_n_genes*i] = fitness;

	barrier(CLK_GLOBAL_MEM_FENCE);
	
	int mod = 1;
	float cases_rem = (float) _n_cases;
	while (cases_rem > 1) {
		if (i%(mod*2) != 0)
			return;
		if (i + mod < _n_cases)
			_eval[_n_genes*i] += _eval[_n_genes*(i+mod)];

		mod *= 2;
		cases_rem *= 0.5;
		barrier(CLK_GLOBAL_MEM_FENCE);
	}

	if (i == 0)
		_fitness[_prog] = _eval[0];

	return;
}

__kernel void scan(__global float* _array, int _n_cases) {
	int i = get_global_id(0);
	
	int n = _n_cases;
	float r = (float) n;
	int mod = 1;

	// UPSWEEP
	while (r > 1) {
		if (i%(mod*2) == 0 && i + mod < n)
			_array[n-1-i] += _array[n-1-i-mod];
		mod *= 2;
		r *= 0.5;
		barrier(CLK_GLOBAL_MEM_FENCE);
	}
	mod /= 2;

	// CLEAR
	float f;
	if (i == 0) {
		f = _array[n-1];
		_array[n-1] = 0.0;
	}

	// DOWNSWEEP
	float t; 
	while (r < n) {
		if ( i%(mod*2) == 0 ) {
			t = _array[n-1-i-mod];
			_array[n-1-i-mod] = _array[n-1-i];
			_array[n-1-i] += t;
		}
		mod /= 2;
		r *= 2;
		barrier(CLK_GLOBAL_MEM_FENCE);
	}

	// SHIFT
	if (i < n-1) {
		t = _array[i+1];
		barrier(CLK_GLOBAL_MEM_FENCE);
		_array[i] = t;
	}
	if (i == 0)
		_array[n-1] = f;

	return;
}
